//遥控船测试 2022——11——03
#include <esp_now.h>
#include <WiFi.h>  //espnow
#include <Wire.h>
#include "SSD1306Wire.h"  //oled
#include <Arduino.h>      //moter
#include <Servo.h>        //Servo
#include <JY901.h>      //姿态传感器
Servo servo;
//电机引脚
#define moter1 26
#define moter2 25
#define moter3 33
#define moter4 32


//IIC——oled引脚            21  22
SSD1306Wire display(0x3c, SDA, SCL);

static const int servoPin = 15;  //舵机引脚

//超声波
const int TrigPin1 = 4;   //Trig
const int EchoPin1 = 2;   //Echo  ---后端
const int TrigPin2 = 13;  //Trig
const int EchoPin2 = 12;  //Echo ---前端
float distance1;          //超声波距离变量
float distance2;

int counter = 1;   //oled计数
int freq = 2000;   //设置PWM波的频率
int channel1 = 4;  //设置通道,共16个通道，0~15
int channel2 = 5;
int channel3 = 2;
int channel4 = 3;

int resolution = 10;  //分辨率，取值0~20 duty的最大值为 2^resolution-1
bool val = 0;

Servo servo6;  //舵机
int SOar = 0;  //舵机角度

int Angle_now = 0;//船身此时刻角度
int Angle_auto = 0;//在开启auto模式时记录船身的角度

int speed_init;//初始化左摇杆油门中间值
int flag_sp_init=0;//油门初始化标志位
/************************************************************************/
//获取图像坐标参量
#define numdata_length 2
String comdata = "";
int numdata[numdata_length] = {0};
int flag = 0;
int x_photo;//x坐标
int y_photo;//y//坐标      
int set_zuobiao;
/************************************************************************/
//PID参数初始化
float time1, duration1, distance_pid;
//PID constants****************************************************************
float setPoint = 0;    //滑轨中心与测距模块的距离
float error;           //当前误差
float errorA; 
float errorD; 
float previous_errorD;  //上一时刻误差，用来计算D
float kp = 5;          //10
float ki = 0;          //0.05
float kd = 0;          //200
int dt = 50;           //每50毫秒进行一次计算
int P, I, D, PID;
int speedL = 0;  //左电机的速度  开闭环共用接口，方便oled监测，后期出事了再改
int speedR = 0;  //右电机的速度
int pid_key = 0;
int error_control=0;//恒速电机的误差控制变量
//*****************************************************************************
//esp——now 创建一个结构体接收数据
// 接收端的MAC地址
uint8_t broadcastAddress[] = { 0xC8, 0xF0, 0x9E, 0xAC, 0xA5, 0x04 };  //C8:F0:9E:AC:A5:04

// 创建一个结构体接收数据
typedef struct struct_message {
  int rx;
  int ry;
  int lx;
  int ly;
  int lok;
  int rok;
  int AUTOjudg;  //自控开关
  //
  int Angle; 
  int sp1;
  int sp2;
  int ml;
  int mr;
  int so;
  int led;
  int control; 
} struct_message;
int connectedjudg = 1;
// 创建一个结构体变量
struct_message myData;

// 回调函数,当收到消息时会调佣该函数
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
  memcpy(&myData, incomingData, sizeof(myData));
  Serial.print("Bytes received: ");
  Serial.println(len);
  Serial.print("a: ");
  Serial.println(myData.rx);
  Serial.print("b: ");
  Serial.println(myData.lx);
  Serial.print("c: ");
  Serial.println(myData.ly);
  Serial.print("d: ");
  Serial.println(myData.ry);
  Serial.println();
}
//ESPnow 发送函数
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  Serial.print("\r\nLast Packet Send Status:\t");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
  if (status == ESP_NOW_SEND_SUCCESS) {
    connectedjudg = 1;
  } else {
    connectedjudg = 0;
  }
}

void setup() {
  Serial.begin(115200);
   JY901.StartIIC();//姿态初始化
  //esp——now初始化
  // 设置wifi模式
  WiFi.mode(WIFI_STA);
  // 初始化esp-now
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  //注册接收信息的回调函数
  esp_now_register_recv_cb(OnDataRecv);
  esp_now_register_send_cb(OnDataSent);
  // 注册通信频道
  esp_now_peer_info_t peerInfo;
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;      //通道
  peerInfo.encrypt = false;  //是否加密为False
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Failed to add peer");
    return;
  }
  //oled初始化
  display.init();
  display.flipScreenVertically();
  display.setFont(ArialMT_Plain_10);
  //左轮
  ledcSetup(channel1, freq, resolution);  //设置通道0
  ledcAttachPin(moter1, channel1);        //将通道0和gpio_pin连接起来
  ledcSetup(channel2, freq, resolution);  //设置通道1
  ledcAttachPin(moter2, channel2);
  //右轮
  ledcSetup(channel3, freq, resolution);  //设置通道3
  ledcAttachPin(moter3, channel3);
  ledcSetup(channel4, freq, resolution);  //设置通道4
  ledcAttachPin(moter4, channel4);
  //舵机
  servo6.attach(servoPin);

  //超声波
  pinMode(TrigPin1, OUTPUT);
  pinMode(EchoPin1, INPUT);
  pinMode(TrigPin2, OUTPUT);
  pinMode(EchoPin2, INPUT);
}


//电机控制函数
// 电机的控制程序，分别是左右两个轮子的占空比（0-1024）
void Motor_Control(int Cnt_L, int Cnt_R) {
  if (Cnt_L >= 0)  // 左轮正向转
  {
    ledcWrite(channel1, Cnt_L);
    ledcWrite(channel2, 0);
  } else  // 左轮反向转
  {
    ledcWrite(channel1, 0);
    ledcWrite(channel2, -Cnt_L);
  }

  if (Cnt_R >= 0)  // 右轮正向转
  {
    ledcWrite(channel3, Cnt_R);
    ledcWrite(channel4, 0);
  } else  // 右轮反向转
  {
    ledcWrite(channel3, 0);
    ledcWrite(channel4, -Cnt_R);
  }
}

//测距函数
void superget(void) {
  digitalWrite(TrigPin1, LOW);
  delayMicroseconds(2);
  digitalWrite(TrigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigPin1, LOW);
  // 检测脉冲宽度，并计算出距离
  distance1 = pulseIn(EchoPin1, HIGH) / 58.00;

  digitalWrite(TrigPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(TrigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigPin2, LOW);
  // 检测脉冲宽度，并计算出距离
  distance2 = pulseIn(EchoPin2, HIGH) / 58.00;


}

void superget_OLED(void) {
  display.clear();
  display.setFont(ArialMT_Plain_10);
  display.drawString(70, 5, "RX:" + String(myData.rx));
  display.drawString(20, 5, "LX:" + String(myData.lx));
  display.drawString(70, 15, "RY:" + String(myData.ry));
  display.drawString(20, 15, "LY:" + String(myData.ly));
  display.drawString(20, 30, "VL:" + String(speedL));
  display.drawString(70, 30, "VR:" + String(speedR));
  display.drawString(70, 40, "SO:" + String(SOar));
  display.drawString(20, 50, String(distance1) + "CM");
  display.drawString(70, 50, String(distance2) + "CM");
  display.display();
}

void JY901get(void) {
  JY901.GetAngle();//获取角度
   // if ((float)JY901.stcAngle.Angle[2]/32768*180>=0)
    Angle_now = (float)JY901.stcAngle.Angle[2]/32768*180 ;//获得基准角度  
    //else Angle_auto = (float)JY901.stcAngle.Angle[2]/32768*180 + 360 ;//获得基准角度  
  // Serial.print("Angle0:");Serial.print(" ");Serial.println(Angle901);
  // Serial.print("Angle1:");Serial.print(" ");Serial.println((float)JY901.stcAngle.Angle[2]/32768*180);
}


void get_photo()//获取图像光圈亮点坐标
{
  int j = 0;
//不断循环检测串口缓存，一个个读入字符串
while (Serial.available() > 0)
{
  comdata += char(Serial.read());
  delay(2);
  flag = 1;
}
//如果接收到数据则执行comdata分析操作，否则什么都不做
if(flag == 1) {
for(int i = 0; i < comdata.length() ; i++){
  if(comdata[i] == ','){
    j++;
    }
  else{
    numdata[j] = numdata[j] * 10 + (comdata[i] - '0');
    }
  }
comdata = String("");
flag = 0;
  x_photo=numdata[0];
  y_photo=(numdata[1]+38)/10;
  numdata[0] = 0;
  numdata[1] = 0;
}
  }


void speed_setinit(void)
{
  if(flag_sp_init == 0 && myData.ly != 0 ){
  int i=0;
  for(i=0;i<=10;i++)
  {
    speed_init = speed_init + myData.ly;
  }
  speed_init = speed_init / 11;
  delay(10);
  flag_sp_init = 1;
}
}

void Speed_Set(void) {
  /* speedL speedR */
  //直线速度
  if (myData.ly > 4000)  //限速
  { myData.ly = 4000; }
  if (myData.ly < speed_init + 200 && myData.ly > speed_init - 600)  //防死区
  {
    speedL = 0;
    speedR = 0;
  } else {
    if (myData.ly <= speed_init - 600) {
      speedL = -(4000 - myData.ly) / 4;  //550-1000
      speedR = -(4000 - myData.ly) / 4;
    }
    if (myData.ly >= speed_init - 600 + 200) {
      speedL = myData.ly / 4;  //550-1000
      speedR = myData.ly / 4;
    }
  }
  //转向
  if (myData.lx > 4000) { myData.lx = 4000; }
  if (myData.lx > 2200 || myData.lx < 1800) {
    if (myData.lx <= 1800) {
      speedL += (4000 - myData.lx) / 20;  //差速290-500
      speedR -= (4000 - myData.lx) / 20;
    }
    if (myData.lx >= 2200) {
      speedL -= myData.lx / 20;  //差速290-500
      speedR += myData.lx / 20;
    }
  }
  if (speedL > -270 && speedL < 270)  //防死区
  { speedL = 0; }
  if (speedR > -270 && speedR < 270) { speedR = 0; }
  if (speedL > 1020)  //限幅
  { speedL = 1020; }
  if (speedR > 1020) { speedR = 1020; }
}
void Servo_Control(void) {
  if (myData.ry > 3000) {
    SOar = 180;
    servo6.write(SOar);
  }
  if (myData.ry < 1000) {
    SOar = 0;
    servo6.write(SOar);
  }
  //AUTO预先控制
  error_control = myData.control+600;
}
void ESPnow_sent(void) {
  myData.sp1 = distance1;  //测距1
  myData.sp2 = distance2;  //测距2
  myData.ml = speedL;
  myData.mr = speedR;
  myData.so = SOar;
  //myData.led = PID;  //灯
  myData.Angle = Angle_now;
  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *)&myData, sizeof(myData));
}



void PID_OLED(void) {
  display.clear();
  display.setFont(ArialMT_Plain_10);
  display.drawString(20, 5, "VL:" + String(speedL));
  display.drawString(70, 5, "VR:" + String(speedR));
  //display.drawString(70, 40, "SO:" + String(SOar));
  display.drawString(20, 20, "^ " + String(distance2) + "CM");
  display.drawString(20, 35, "v " + String(distance1) + "CM");
  //pid参数
  display.drawString(80, 20, "KP:" + String(P));
  display.drawString(80, 35, "kI:" + String(I));
  display.drawString(80, 50, "ER:" + String(error));
  display.drawString(20, 50, "MODE:" + String(pid_key));
  display.display();
}



void AUTO_Control(void) {
    if(myData.AUTOjudg == 1){
    JY901.GetAngle();//获取角度 
    get_photo();
   // if ((float)JY901.stcAngle.Angle[2]/32768*180>=0)
    Angle_auto = (float)JY901.stcAngle.Angle[2]/32768*180 ;//获得基准角度  
   // else Angle_auto = (float)JY901.stcAngle.Angle[2]/32768*180 + 360 ;//获得基准角度  
    }
    speedL = 600;//PID电机
    speedR = 650;//定速电机
  while (myData.AUTOjudg == 1) {
    /////////////////////////////////////////////////////////////////////
    
    superget();  //超声波获取距离
    JY901.GetAngle();//获取角度 
    
    Angle_now = (float)JY901.stcAngle.Angle[2]/32768*180;//获得实时角度  
    ESPnow_sent();  //回传信息至遥控器
      speedL = 800;
    errorA = -Angle_auto + Angle_now ; //计算角度误差
    P = 20 * errorA;  //P项
    PID = P ;                           //PID
    if (PID > 100) { PID = 100; }              //限幅
    if (PID < -100) { PID = -100; }            //限幅
     if(errorA<=3&&errorA>=-3){
    errorD = distance1 - 166;      //计算误差
     P = 4 * errorD  ;  //P项
     D = 15 * ((errorD - previous_errorD) / dt);
     PID = -P ;                           //PID
     if (PID > 100) { PID = 100; }              //限幅
     if (PID < -100) { PID = -100; }            //限幅
     previous_errorD = errorD;  //更新上一时刻误差
     }

      speedL += 1.3*PID;
      speedR = 800;
    
    Motor_Control(speedL, speedR);
    delay(50);
    ////////////////////////////////////////////////////////////////////
    if (myData.AUTOjudg == 0) break;
  }
}

void Lose_Control(void) {
  while (connectedjudg == 0) {
    ESPnow_sent();        //回传信息至遥控器
    Motor_Control(0, 0);  //电机休止
    display.clear();
    display.setFont(ArialMT_Plain_24);  //字体大小
    display.drawString(2, 15, "Un Connected");
    display.display();
    if (connectedjudg == 1) break;
  }
}

//主函数
void loop() {
  
  AUTO_Control();                 //自控
  JY901get();                     //获取角度
  superget();                     //超声波获取距离
  superget_OLED();                //oled
  Servo_Control();                //舵机
  speed_setinit();                //油门校准
  Speed_Set();                    //PID
  Motor_Control(speedL, speedR);  //电机输出
  ESPnow_sent();                  //回传信息至遥控器
  Lose_Control();                 //失控判断
}